/*
 * Copyright (c) 2022 Huawei Device Co., Ltd.
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/////. Start CORDIC
class MathCordic {
    public static final double FI_CONST = 65536.0;
    public static final double DEG_CONST = 0.017453;
    public static final double AG_CONST = 0.607252935;
    public static final double ANGLE_1_CONST = 45.0;
    public static final double ANGLE_2_CONST = 26.565;
    public static final double ANGLE_3_CONST = 14.0362;
    public static final double ANGLE_4_CONST = 7.12502;
    public static final double ANGLE_5_CONST = 3.57633;
    public static final double ANGLE_6_CONST = 1.78991;
    public static final double ANGLE_7_CONST = 0.895174;
    public static final double ANGLE_8_CONST = 0.447614;
    public static final double ANGLE_9_CONST = 0.223811;
    public static final double ANGLE_10_CONST = 0.111906;
    public static final double ANGLE_11_CONST = 0.055953;
    public static final double ANGLE_12_CONST = 0.027977;
    public static final int STEP_CONST = 12;
    public static final int CORDIC_CONST = 25000;
    public static final int NUM_1000_CONST = 1000;
    public static final int NUM_TIME_LOOP1_CONST = 80;
    public static double fixed(double x) {
        return x * FI_CONST;
    }

    public static double float1(double x) {
        return x / FI_CONST;
    }

    public static double deg2rad(double x) {
        return DEG_CONST * x;
    }

    public static double[] angles = {
            fixed(ANGLE_1_CONST),
            fixed(ANGLE_2_CONST),
            fixed(ANGLE_3_CONST),
            fixed(ANGLE_4_CONST),
            fixed(ANGLE_5_CONST),
            fixed(ANGLE_6_CONST),
            fixed(ANGLE_7_CONST),
            fixed(ANGLE_8_CONST),
            fixed(ANGLE_9_CONST),
            fixed(ANGLE_10_CONST),
            fixed(ANGLE_11_CONST),
            fixed(ANGLE_12_CONST)
    };

    public static double target = 28.027;

    public static double cordicsincos(double target) {
        double x;
        double y;
        double targetAngle;
        double currAngle;

        x = fixed(AG_CONST); /* AG_CONST * cos(0) */
        y = 0; /* AG_CONST * sin(0) */

        targetAngle = fixed(target);
        currAngle = 0;

        for (int step = 0; step < STEP_CONST; step++) {
            double newX;
            if (targetAngle > currAngle) {
                newX = x - ((int) y >> step);
                y = ((int) x >> step) + y;
                x = newX;
                currAngle += angles[step];
            } else {
                newX = x + ((int) y >> step);
                y = -((int) x >> step) + y;
                x = newX;
                currAngle -= angles[step];
            }
        }
//        System.out.println("FLOAT(X) = " + float1(x) + " , FLOAT(Y) = " + float1(y));
        return float1(x) * float1(y);
    }

    ///// End CORDIC
    public static double total = 0.0;

    public static double cordic(int runs) {
        double start = System.nanoTime() / 1000_000.0;
        for (int i = 0; i < runs; i++) {
            total += cordicsincos(target);
        }
        double end = System.nanoTime() / 1000_000.0;
        return end - start;
    }
}

/*
 *  @State
 *  @Tags Jetstream2
 */
class Benchmark {
    /*
     *  @Benchmark
     */
    public static void run() throws Exception {
        MathCordic.cordic(MathCordic.CORDIC_CONST);
        double expected = 10362.570468755888;
        if (MathCordic.total != expected) {
            throw new Exception("ERROR: bad result: expected  (expected)   but got (total)");
        }
    }

    /**
     * @Benchmark
     */
    public static void runIterationTime() throws Exception {
        double start = System.nanoTime() / 1000_000.0;
        for (int i = 0; i < MathCordic.NUM_TIME_LOOP1_CONST; i++) {
            MathCordic.total = 0.0;
            run();
        }
//        System.out.println("tatal = " + MathCordic.total);
        double end = System.nanoTime() / 1000_000.0;
        System.out.println("math-cordic: ms = " + (end - start));
    }

    public static void main(String[] args) throws Exception {
        runIterationTime();
    }
}

